/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package file;

import com.sun.squawk.util.StringTokenizer;

/**
 *
 * @author Mentors
 */
public class ScriptVerifier {
    
    AutonomousScript autoScript;
    
    String  autoCommandLine;
    String  autoCommand;

    boolean followingLine   = false;
    boolean autoCommandBusy = false;
    
    int     autoHeading;
    double  autoSpeed;
    int     autoDistance;
    double  autoTime;
    double  autoGain;
    int     autoProximity;
    double  autoDelay;  // interval to "delay" from autonomous script command
    
    public ScriptVerifier () {

        autoScript = AutonomousScript.getInstance();

    }

    public void testScript()
    {
        while (autoScript.hasNextCommand() == true) {

            if (autoCommandBusy == false) {

                autoCommandLine = autoScript.getNextCommand();

                StringTokenizer st = new StringTokenizer(autoCommandLine);
                autoCommand = st.nextToken();
                System.out.println("Autonomous command " + autoCommand);

                if (autoCommand.equalsIgnoreCase("FollowLine")) {
                    followingLine = processFollowLineCommand(st);
                    autoCommandBusy = false;
                }

                else if(autoCommand.equalsIgnoreCase("Drive")) {
                    autoCommandBusy = processDriveCommand(st);
                }

                else if (autoCommand.equalsIgnoreCase("MoveElevator")) {
                    autoCommandBusy = processMoveElevatorCommand(st);
                }

                else if (autoCommand.equalsIgnoreCase("PlaceOnPeg")) {
                    autoCommandBusy = processPlaceOnPegCommand(st);
                }

                else if (autoCommand.equalsIgnoreCase("Delay")) {
                    autoCommandBusy = processDelayCommand(st);
                }

                else if (autoCommand.equalsIgnoreCase("Halt")) {
                    // nothing to do.  Should be the last commnad in a script so
                    // autoScript.hasNextCommand() should return false
                }

                else {
                    System.out.println("Invalid script command - " + autoCommand);
                    autoCommandBusy = false;
                }
                autoCommandBusy = false;
            }
        }
    }

    private boolean processFollowLineCommand(StringTokenizer st) {

        boolean  validCommand = true;   // assume no errors
        String[] param;                 // holds param name and value

        String   path      = "";        // empty string if param not found
        String   speed     = "";        // empty string if param not found
        String   gain      = "";        // empty string if param not found
        String   proximity = "";        // empty string if param not found

        // get type, speed, gain, proximity

        int      numberOfParams = st.countTokens();

        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("path")) {
                path = param[1];

            } else if (param[0].equalsIgnoreCase("speed")) {
                speed = param[1];

            } else if (param[0].equalsIgnoreCase("gain")) {
                gain = param[1];

            } else if (param[0].equalsIgnoreCase("proximity")) {
                proximity = param[1];

            } else {
                System.out.println("Invalid autoscript parameter (FollowLine): " + param[0]);
            }
            System.out.println("Follow line param[0] " + param[0] + " " + "Follow line param[1] " + param[1]);
        }

        try
        {
            autoSpeed     = Double.parseDouble( speed );
            autoGain      = Double.parseDouble( gain );
            autoProximity = Integer.parseInt( proximity );

            if (path.equalsIgnoreCase("straight") || path.equalsIgnoreCase("left") || path.equalsIgnoreCase("right")) {
                System.out.println("followLine " + path + "" + autoSpeed + "" + autoGain + "" + autoProximity);

            } else {
                System.out.println("Invalid autoscript param value (FollowLine): " + path);
            }
        }
        catch (NumberFormatException e)
        {
            System.out.println("Invalid autoscript parameter value (FollowLine)");
            validCommand = false;
        }
        return validCommand;
    }


    private boolean processDriveCommand (StringTokenizer st) {

        boolean  validCommand = true;   // assume no errors
        String[] param;                 // holds param name and value

        String   speed     = "";        // empty string if param not found
        String   heading   = "";        // empty string if param not found
        String   distance  = "";        // empty string if param not found
        String   time      = "";        // empty string if param not found

        // get speed, heading, distance, time

        int      numberOfParams = st.countTokens();

        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("speed")) {
                speed = param[1];

            } else if (param[0].equalsIgnoreCase("heading")) {
                heading = param[1];

            } else if (param[0].equalsIgnoreCase("distance")) {
                distance = param[1];

            } else if (param[0].equalsIgnoreCase("time")) {
                time = param[1];

            } else {
                System.out.println("Invalid autoscript parameter (Drive): " + param[0]);
            }
        }

        try
        {
            autoSpeed     = Double.parseDouble( speed );
            autoHeading   = Integer.parseInt( heading );

            if (distance.equalsIgnoreCase("")) {
                autoDistance = 0;
            } else {
                autoDistance = Integer.parseInt( distance );
            }

            if (time.equalsIgnoreCase("")) {
                autoTime = 0.0;
            } else {
                autoTime = Double.parseDouble( time );
            }

            if (distance.equalsIgnoreCase("") && time.equalsIgnoreCase("")) {
                System.out.println("Invalid autoscript param value (Drive): ");
//                lineTracker.start(path, autoSpeed, autoGain, autoProximity);

            } else {

                System.out.println("drive " + autoSpeed + " " + autoHeading + " " + 0);
            }
        }
        catch (NumberFormatException e)
        {
            System.out.println("Invalid autoscript parameter value (Drive)");
            validCommand = false;
        }
        return validCommand;
    }


    private boolean processMoveElevatorCommand (StringTokenizer st) {

        boolean  validCommand     = true;   // assume no errors
        String[] param;                     // holds param name and value
        String   elevatorPosition = "";     // empty string if param not found
        boolean  elevatorOffset   = true;   // default to higher peg

        // get position and offset parameters

        int numberOfParams = st.countTokens();

        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("position")) {
                elevatorPosition = param[1];

            } else if (param[0].equalsIgnoreCase("offset")) {
                elevatorOffset = param[1].equalsIgnoreCase("true") ? true : false;
            }
            else {
                System.out.println("Invalid autoscript param (MoveElevator): " + param[0]);
            }
        }

        System.out.println("move elevator " + elevatorPosition + " " + elevatorOffset);

        if (elevatorPosition.equalsIgnoreCase("top")) {
//            elevator.moveToTopPeg(elevatorOffset);

        } else if (elevatorPosition.equalsIgnoreCase("middle")) {
//            elevator.moveToMiddlePeg(elevatorOffset);

        } else if (elevatorPosition.equalsIgnoreCase("bottom")) {
//            elevator.moveToBottomPeg(elevatorOffset);

        } else if (elevatorPosition.equalsIgnoreCase("feeder")) {
//            elevator.moveToFeeder();

        } else {
            System.out.println("Invalid autoscript param value (MoveElevator): " + elevatorPosition);
            validCommand = false;
        }

        return validCommand;
    }


    private boolean processPlaceOnPegCommand(StringTokenizer st) {

        // wait until robot is in position and elevator is in position


//        if lineTracker.atEnd() {
//            if elevator is in position {
//                move gripper and release
//                autoCommandBusy = false;
//            }
//        }

        return true;
    }


    private boolean processDelayCommand(StringTokenizer st) {

        boolean  validCommand = true;   // assume no errors
        String[] param;                 // holds param name and value
        String   seconds      = "";     // empty string if param not found

        // get seconds parameter

        int numberOfParams = st.countTokens();

        for (int i = 0; i  < numberOfParams; i++) {

            param = getParam(st.nextToken());

            if (param[0].equalsIgnoreCase("seconds") || param[0].equalsIgnoreCase("secs")) {
                seconds = param[1];
            }
            else {
                System.out.println("Invalid autoscript param (Delay): " + param[0]);
            }
        }

        try
        {
            autoDelay = Double.parseDouble( seconds );

            System.out.println("Delay " + autoDelay + " seconds.");
        }
        catch (NumberFormatException e)
        {
            System.out.println("Invalid autoscript param value (Delay): " + seconds);
            validCommand = false;
        }

        return validCommand;
    }


    private String[] getParam(String namedParam) {

        int      equalSignIndex = namedParam.indexOf("=");
        String[] param;

	//to store the parameter name and value
	param = new String[2];

        if (equalSignIndex != -1) {
            param[0] = namedParam.substring(0, equalSignIndex);
            param[1] = namedParam.substring(equalSignIndex + 1, namedParam.length());
        }  else {
            param[0] = "";
            param[1] = "";
        }

        return param;
    }
}
